#include "rclcpp/rclcpp.hpp"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/passthrough.h>
#include <pcl/make_shared.h> 

#include <chrono>
#include <iomanip>
#include <sstream>
#include <string>
#include <limits>

class PcdIntensityFilter : public rclcpp::Node
{
public:
    using PointT = pcl::PointXYZI;
    using PointCloud = pcl::PointCloud<PointT>;

    explicit PcdIntensityFilter(const std::string &name) : Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "节点 %s: 已启动.", name.c_str());

        // --- 1. 获取参数 ---
        this->declare_parameter<std::string>("input_pcd_file", "");
        std::string input_file = this->get_parameter("input_pcd_file").as_string();

        this->declare_parameter<std::string>("output_directory", "");
        std::string output_dir = this->get_parameter("output_directory").as_string();

        this->declare_parameter<double>("intensity_threshold", 50.0);
        double intensity_threshold = this->get_parameter("intensity_threshold").as_double();

        RCLCPP_INFO(this->get_logger(), "读取文件: %s", input_file.c_str());
        RCLCPP_INFO(this->get_logger(), "滤波后pcd文件保存目录: %s", output_dir.c_str());
        RCLCPP_INFO(this->get_logger(), "强度阈值: %f", intensity_threshold);

        if (input_file.empty() || output_dir.empty())
        {
            RCLCPP_ERROR(this->get_logger(), "输入文件路径或输出文件夹无效，节点将关闭。");
            return; // 提前返回
        }

        // --- 2. 加载点云 ---
        PointCloud::Ptr input_cloud = pcl::make_shared<PointCloud>();
        if (pcl::io::loadPCDFile<PointT>(input_file, *input_cloud) == -1)
        {
            RCLCPP_ERROR(this->get_logger(), "无法读取输入pcd文件: %s", input_file.c_str());
            return;
        }
        RCLCPP_INFO(this->get_logger(), "原始点云点数: %ld", input_cloud->size());

        // --- 3. 执行滤波 ---
        PointCloud::Ptr filtered_cloud = pcl::make_shared<PointCloud>();
        pcl::PassThrough<PointT> pass;
        pass.setInputCloud(input_cloud);
        pass.setFilterFieldName("intensity");
        pass.setFilterLimits(intensity_threshold, std::numeric_limits<float>::max()); // PointXYZI的强度是float类型
        pass.filter(*filtered_cloud);
        RCLCPP_INFO(this->get_logger(), "滤波后点云点数: %ld", filtered_cloud->size());

        // --- 4. 保存滤波后的点云 ---
        auto now = std::chrono::system_clock::now();
        auto now_c = std::chrono::system_clock::to_time_t(now);
        std::stringstream ss;
        ss << "filtered_point_cloud_" << std::put_time(std::localtime(&now_c), "%Y-%m-%d_%H-%M-%S");
        std::string file_name = ss.str() + ".pcd";

        if (output_dir.back() != '/')
        {
            output_dir += '/';
        }
        std::string full_path = output_dir + file_name;

        if (pcl::io::savePCDFileASCII(full_path, *filtered_cloud) == 0)
        {
            RCLCPP_INFO(this->get_logger(), "成功保存pcd文件: '%s'", full_path.c_str());
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "保存pcd文件失败: '%s'", full_path.c_str());
        }
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<PcdIntensityFilter>("pcd_intensity_filter");
    rclcpp::shutdown();
    return 0;
}